
#ifndef _BIT_BUF_H_
#define _BIT_BUF_H_

#include <stdint.h>
#include "BitDefine.h"

struct BitBundle{
	uint32_t bundle;
	uint8_t num;
	uint8_t off;
};

typedef struct BitBundle BitBundle_T;

/* Number of bits represented by one bundle */
#define bundle_bitness(ba)	(sizeof(ba->bundles[0]) * 8)

struct BitBuf {
	/* Number of bits */
	uint32_t num_bits;

	/* Number of bundles */
	uint32_t num_bundles;

	/* Bundle of bits */
	uint32_t *bundles;
};

typedef struct BitBuf BitBuf_T;

#define BITBUF_DEFINE(name, total_bits)				\
	uint32_t _BitBuf_bundles_##name				\
		[(((total_bits + 8 - 1) / 8) + sizeof(uint32_t) - 1)	\
		 / sizeof(uint32_t)] = {0U};				\
	BitBuf_T name = {						\
		.num_bits = total_bits,					\
		.num_bundles = (((total_bits + 8 - 1) / 8)		\
				+ sizeof(uint32_t) - 1)			\
			       / sizeof(uint32_t),			\
		.bundles = _BitBuf_bundles_##name,		\
	}


#define BUNDLE_BIT_NUM		(sizeof(uint32_t)*8)
#define BUNDLE_MASK			(BUNDLE_BIT_NUM - 1)
#define BundleOffset(pos)		((pos) >> 5)
#define BitOffset(pos)			((pos) & BUNDLE_MASK)

#define BitBundle_Clear(bundle, bit_num) ((bundle) &= ~ Bit(bit_num))
#define BitBundle_Set(bundle, bit_num) ((bundle) |= Bit(bit_num));
#define BitBundle_Get(bundle, bit_num) (((uint32_t)(bundle) >>(bit_num)) & 1u)
#define BitBundle_Test(bundle, bit_num)	((bundle & Bit(bit_num)) != 0 )

#define BitBundle_Up(bundle, start)		Bits_Get(bundle, start, 31)
#define BitBundle_Down(bundle, start)	Bits_Get(bundle, 0, start)
#define BitBundle_Range(bundle, start, end)		Bits_Get(bundle, start, end)

static inline void BitBundles_Write(uint32_t* bundles, const uint32_t pos, uint8_t val){
	const uint32_t idx = BundleOffset(pos);
	const uint32_t bit = BitOffset(pos);
	if(val){
		BitBundle_Set(bundles[idx], bit);
	}
	else{
		BitBundle_Clear(bundles[idx], bit);
	}
}


static inline int BitBundles_WriteSnap(uint32_t* bundles, uint32_t pos, uint32_t snap, uint32_t num, uint32_t off){
	uint32_t idx = BundleOffset(pos);
	uint32_t bit = BitOffset(pos);
	num = num < (BUNDLE_BIT_NUM - off) ? num : (BUNDLE_BIT_NUM - off);

	if(num == 0){
		return 0;
	}
	
	const uint32_t end = pos + num - 1;
	const uint32_t eidx = BundleOffset(end);
	const uint32_t ebit = BitOffset(end);
	
	if(idx == eidx){
		uint32_t val = BitBundle_Range(snap, off, num + off);
		bundles[idx] &= ~ Bits(bit, ebit);
		bundles[idx] |= val << bit;
	}
	else{
		uint32_t val = BitBundle_Range(snap, off, num + off);
		bundles[idx] &= ~ Bits(bit, 31);
		bundles[idx] |= (uint32_t)(val << bit);
		bundles[eidx] &= ~ Bits(0, ebit);
		bundles[eidx] |= (uint32_t)(val >> (BUNDLE_BIT_NUM - bit));
	}
	
	return num;
}



static inline int BitBundles_Read(const uint32_t* bundles, const uint32_t pos){
	const uint32_t idx = BundleOffset(pos);
	const uint32_t bit = BitOffset(pos);
	
	return BitBundle_Get(bundles[idx], bit);
}


static inline int BitBundles_ReadSnap(const uint32_t* bundles, uint32_t pos, uint32_t* snap, uint32_t num, uint32_t off){
	uint32_t idx = BundleOffset(pos);
	uint32_t bit = BitOffset(pos);

	num = num < (BUNDLE_BIT_NUM - off) ? num : (BUNDLE_BIT_NUM - off);

	if(num == 0){
		return 0;
	}

	const uint32_t end = pos + num - 1;
	const uint32_t eidx = BundleOffset(end);
	const uint32_t ebit = BitOffset(end);
	
	if(idx == eidx){
		uint32_t val;
		val = BitBundle_Range(bundles[idx], bit, ebit);
		*snap |= val << off;
	}
	else{
		uint32_t val, val2;
		val = BitBundle_Up(bundles[idx], bit);
		val2 = BitBundle_Down(bundles[eidx], ebit);
		val |= val2 << (BUNDLE_BIT_NUM - bit);
		*snap |= val << off;
	}
	
	return num;
}


static inline int BitBuf_CheckPos(const BitBuf_T *bitbuf, const uint32_t pos){
	return pos < bitbuf->num_bits;
}

static inline void BitBuf_Write(BitBuf_T *bitbuf, uint32_t pos, uint8_t val){
	uint32_t idx = BundleOffset(pos);
	uint32_t bit = BitOffset(pos);
	if(val){
		BitBundle_Set(bitbuf->bundles[idx], bit);
	}
	else{
		BitBundle_Clear(bitbuf->bundles[idx], bit);
	}
}

static inline int BitBuf_Read(const BitBuf_T *bitbuf, uint32_t pos){
	const uint32_t idx = BundleOffset(pos);
	const uint32_t bit = BitOffset(pos);
	
	return BitBundle_Get(bitbuf->bundles[idx], bit);
}


void BitBuf_Init(BitBuf_T* bitbuf, uint32_t *bundle, uint32_t total_bits);

int BitBuf_SetBit(BitBuf_T *bitbuf, uint32_t bit);

int BitBuf_ClearBit(BitBuf_T *bitbuf, uint32_t bit);

int BitBuf_TestBit(BitBuf_T *bitbuf, uint32_t bit, int *val);

int BitBuf_GetBundle(BitBuf_T* bitbuf, uint32_t bit, BitBundle_T* bitbundle);

int BitBuf_SetBundle(BitBuf_T* bitbuf, uint32_t bit, BitBundle_T* bitbundle);

#endif

